\section{Построение фильтра Калмана-Бьюси}
Нашей целью в данном разделе является построение непрерывного аналога для дискретного фильтра Калмана, с которым мы познакомились в прошлом семестре. Начнем наше построение с формулировки задачи.

\subsection{Постановка задачи}
Рассмотрим систему с линейными уравнениями динамики и наблюдения
\begin{equation}
	\left\{ \begin{aligned}
		dx & = Ax dt + dv \\
		dy & = Cx dt + de,
	\end{aligned} \right.
	\label{main_task}
\end{equation}
где $x(t_{0}), e(t), v(t)$ независимы в совокупности.

Вектор начального состояния такой, что $\E x(t_{0}) = m_{0}, \Var x(t_{0}) = R_{0}$.

Случайные процессы помех $v(t)$ и $e(t)$ имеют независимые приращения и удовлетворяют свойствам
\begin{align}
	\E e(t) = \E v(t) = 0, \\
	\E de (de)^{T} = R_{1} dt,\ \E dv (dv)^{T} = R_{2} dt.
\end{align}

{\bf Задача}: Пусть на $[t_{0}, t_{1}]$ непрерывно наблюдаются значения $y$. Найти линейную оценку для состояния $x$, оптимальную в среднем квадратическом смысле (ЛООСК), обозначаемую как $\hat{X}$. 
\newline

Мы будем решать немного модифицированную задачу, которая потом сведется к изначальной. А именно, будем оценивать $a^{T} \hat{X}$.  Где $a^{T}$ --- произвольный вектор из $\real^{n}$.

Следовательно, функционалом в модифицированной задаче будет
\begin{equation}
	\E (a^{T} X - a^{T} \hat{X})^2 \rightarrow \min_{u \in C[t_{0}, t_{1}], b \in \real^{n}}.
\end{equation}

Опишем общую схему решения поставленной задачи. Ищем решение поставленной задачи в виде
\begin{equation}
	a^{T} \hat{X} = - \int^{t_{0}}_{t_{1}} u^{T}(s) d y(s)  +  b^{T} m_{0},
\end{equation}
где неизвестны $u$ --- непрерывная на $[t_{0}, t_{1}]$ функция и параметр $b \in \real^{n}$, а минус поставлен для удобства.

\subsection{Подход к решению}
К решению данной задачи подойдем следующим образом:
\begin{enumerate}
	\item Будем искать решение задачи в линейном виде  (т.к. ЛООСК), что сведет задачу к поиску функции $u(t)$ и параметра $b$.
	\item Запишем двойственную задачу, которая не будет включать стохастических членов.
	\item Выразим $a^T X_{t_{1}}$ и $a^T \hat{X}_{t_{1}}$ через двойственную переменную.
	\item Запишем функционал, выразив все члены через двойственную переменную. 
	\item Из вида функционала в явном виде получим оптимальное значение параметра $b$, это сведет задачу оптимизации по $u(t)$ к стандартной детерминированной задаче динамического программирования.
	\item Применим метод динамического программирования, записав уравнение ГЯБ для нашей линейно-квадратичной задачи.
	\item Поиск решения в виде квадратичной форме приведет нас к уравнению Рикатти, решать которое можно, например, численно.
	\item Выпишем оптимальное значение $u$, итоговый вид диффура для ЛООСК $\hat{X}_{t}$, а также дисперсии оценки через матрицу $P$ из уравнения Рикатти.
\end{enumerate}

\subsection{Подробные выкладки}
\subsubsection{Переход к задаче динамического программирования}
Продемонстрируем, что задача идентификации двойственна некоторой задаче оптимального управления. Рассмотрим уравнение
\begin{equation}
	\left\{ \begin{aligned}
		\frac{dz}{dt} &= -A^T z - C^T u, \;\; t \in [t_0, t_1], \\
		z(t_1) &= a
	\end{aligned} \right.
	\label{dual_problem}
\end{equation}
%Очень странный спопоб записать систему уравнений. Обычно это делается так:
%$$
%\begin{cases}
% Ax+b =c,\\
% Dx+f = e.
%\end{cases}
%
%
Выразим $a^T X_{t_{1}}$ через двойственную переменную:
\begin{multline*}
	a^T X_{t_1} = z^T (t_1) X_{t_1} = z^T(t_0) X(t_0) + \int\limits_{t_0}^{t_1} d[z^T (t) X(t)] = \\
	= \text{\{дифференцируем произведение, учитываем \eqref{dual_problem} и уравнение динамики в \eqref{dual_problem}\}} = \\
	= \text{\{приводим однородные члены $\pm z^T A X dt$\}} = z^T(t_0) X(t_0) - \int\limits_{t_0}^{t_1} u^T(t) C(t) X(t) dt + \int\limits_{t_0}^{t_1} z^T(t) dv(t),
\end{multline*}
Выразим $a^T \hat{X}_{t_{1}}$ через двойственную переменную:
\begin{multline*}
	a^T \hat{X}_{t_1} = - \int\limits^{t_{0}}_{t_{1}} u^{T}(s) d y(s)  +  b^{T} m_{0} = - \int\limits_{t_0}^{t_1} u^T(t) C(t) X(t) dt -  \int\limits_{t_0}^{t_1} u^T(t) d e(t) +  b^{T} m_{0}.
\end{multline*}
Матожидание разности будет иметь вид (для справки): 
\begin{multline*}
	\E (a^{T} X - a^{T} \hat{X}) = \E(z^T (t_0) X(t_0) - b^T m_0 + \int\limits_{t_0}^{t_1} z^T(t) dv(t) + \int\limits_{t_0}^{t_1} u^T(t) d e(t) ) = \\
	= \text{\{по свойствам интеграла Ито\}} = z^T (t_0) X(t_0) - b^T m_0
\end{multline*}
% Понять, зачем писали это и откуда вдруг взялась формула для матожидания квадрата???

Теперь мы можем записать функционал через двойственную переменную.
\begin{multline*}
	\E (a^{T} X - a^{T} \hat{X})^2 = z^T(t_0) R_0 z(t_0) + ((z^T(t_0) - b^T)m_0)^2 + \\
	+ \int\limits_{t_0}^{t_1} z^T(t) R_1 z(t) dt + \int\limits_{t_0}^{t_1} u^T(t) R_2 u(t) dt \rightarrow \min_{u \in C[t_{0}, t_{1}], b \in \real^{n}}.
\end{multline*}
Из вида функционала сразу определяется оптимальное значение параметра: $b = z(t_0)$. Теперь нам осталось провести минимизацию по $u(t)\in C[t_{0}, t_{1}]$. 

Уравнение \eqref{dual_problem} вместе с данным функционалом образуют стандартную линейно-квадратичную задачу оптимального управления, решаемую с помощью метода динамического программирования:
\begin{equation*}
	\left\{ \begin{aligned}
		&\frac{dz}{dt} = -A^T z - C^T u, \;\; t \in [t_0, t_1], \\
		&z(t_1) = a, \\
		&z^T(t_0) R_0 z(t_0) + \int\limits_{t_0}^{t_1} z^T(t) R_1 z(t) dt + \int\limits_{t_0}^{t_1} u^T(t) R_2 u(t) dt \rightarrow \min_{u \in C[t_{0}, t_{1}]}.
	\end{aligned} \right.
\end{equation*}

\subsubsection{Решение задачи динамического программирования}
Запишем для поставленной задачи функцию цены
\begin{equation*}
	V(t_1,z_1) = \min_{u} \left\{ z^T(t_0) R_0 z(t_0) + \int\limits_{t_0}^{t_1} z^T(t) R_1 z(t) dt + \int\limits_{t_0}^{t_1} u^T(t) R_2 u(t) dt \middle| z(t_1) = z_1 \right\}.
\end{equation*}

Общее уравнение Гамильтона Якоби Беллмана
\begin{equation*}
\left\{ \begin{aligned}
 	&\frac{\partial V}{\partial t} + \max_u  \left\{ \left< \frac{\partial V}{\partial x}, f(t,x,u) \right> - \mathcal{L} (t,x,u) \right\} = 0, \\
 	&V(t_0,x) = \varphi_0(t_0, x), \text{ где} \\
 	&V(t_1, x_1) = \min_u \left\{ \varphi_0(t_0,x_0) + \int\limits_{t_0}^{t_1} \mathcal{L} (t,x,u) dt \middle| x(t_1) = x_1 \right\}.
\end{aligned} \right.
\end{equation*}
в данной задаче примет вид
\begin{equation*}
\left\{ \begin{aligned}
	&\frac{\partial V}{\partial t} + \max_u  \left\{ \left< \frac{\partial V}{\partial z}, -A^T z - C^T u \right> - z^T(t) R_1 z(t) - u^T(t) R_2 u(t) \right\} = 0, \\
	&V(t_0, z) = z^T R_0 z.
\end{aligned} \right.
\end{equation*}

Из уравнения ГЯБ выделим внутреннюю задачу минимизации 
\begin{equation*}
	\min_u  \left\{ \left< \frac{\partial V}{\partial z}, C^T u \right> + u^T(t) R_2 u(t) \right\}.
\end{equation*}
Так как ограничения отсутствуют, мы можем с помощью дифференцирования выразить оптимальное значение $u(t)$ через частную производную функции цены
\begin{equation*}
	C \frac{\partial V}{\partial z} + 2 R_2 u = 0 \;\; \Rightarrow \;\; u = - \frac{1}{2} R_2^{-1} C \frac{\partial V}{\partial z}
\end{equation*}
Подставим полученное значение $u(t)$ в уравнение ГЯБ. После небольших упрощений, оно преобразуется к следующему виду
\begin{equation}
\left\{ \begin{aligned}
	&\frac{\partial V}{\partial t} - \left< \frac{\partial V}{\partial z}, -A^T z \right> + \frac14 \left< \frac{\partial V}{\partial z}, C^T R_2^{-1} C \frac{\partial V}{\partial z} \right> - z^T R_1 z = 0, \\
	&V(t_0, z) = z^T R_0 z.
\end{aligned} \right.
\label{temp1}
\end{equation}

Из курса по динамическому программированию, мы знаем, что искать функцию цены для данной задачи необходимо в виде квадратичной формы
\begin{equation*}
	V(t,z) = \left< z, P(t) z \right>,  \;\; P = P^T > 0.
\end{equation*}
Подставим данное представление в полученное уравнение ГЯБ \eqref{temp1}
\begin{equation*}
	z^T \dot{P} z - \left< 2Pz, A^T z \right> - z^T R_1 z + \frac14  \left< 2Pz, C^T R_2^{-1} C 2 P z \right> = 0,
\end{equation*}
откуда получим матричное уравнение Рикатти
\begin{equation*}
\left\{ \begin{aligned}
	&\dot{P} - PA^T + AP + P C^T R_2^{-1} P - R_1 = 0, \\
	&P(t_0) = R_0.
\end{aligned} \right.	
\end{equation*}
Из курса динамического программирования известно, что уравнение имеет единственное решение $P = P^T > 0$, которое можно найти, например, численно.

Итак, мы получили решение линейно-квадратичной задачи, для которого управление является линейной обратной связью
\begin{equation*}
	u(t) = -K^T(t)z(t), \;\; \text{где} \;\; K(t) = P(t) C^T(t) R_2^{-1}
\end{equation*}

\subsubsection{Получение решения исходной задачи фильтрации}
Проведя полное решение задачи динамического программирования, мы будем знать матрицу $P(t) \Rightarrow K(t)$ . Это позволяет нам выписать решение модифицированной задачи фильтрации в следующем виде
\begin{equation}
	a^{T} \hat{X}(t_1) = \int\limits_{t_{0}}^{t_{1}} z^{T}(s) K(s) d y(s)  +  z^{T}(t_0) m_{0},	
	\label{mod_sol_tmp}
\end{equation}

Обозначим за $\psi(t,t_1)$ фундаментальную матрицу для нашей двойственной задачи (учтем найденный нами вид для $u(t)$)
\begin{equation*}
\left\{ \begin{aligned}
	&\dot{z}(t) = -A^T z - C^T u = -A^Tz - C^T K^T z = -(A-KC)^T z, \\
	&z(t_1) = a.
\end{aligned} \right.	
\end{equation*}
Тогда двойственную переменную можно записать через фундаментальную матрицу
\begin{equation*}
	z(t) = \psi^T(t_1,t) a
\end{equation*}

Подставим такое выражение в решение модифицированной задачи фильтрации \eqref{mod_sol_tmp}, получим
\begin{equation*}
	a^T \hat{X}(t_1) = a^T \left( \int\limits_{t_{0}}^{t_{1}} \psi(t_1,s) K(s) d y(s)  +  \psi(t_1,t_0) m_{0} \right), \;\; \forall a
\end{equation*}



Так как при формулировке модифицированной задачи мы взяли произвольный вектор $a$, то теперь у нас появилась возможность от него избавиться и получить решения исходной задачи фильтрации. Кроме того, из принципа оптимальности следует, что мы можем заменить в формуле конечный момент $t_1$ на произвольный промежуточный момент $t\in[t_0, t_1]$. Итак, ЛООСК будет иметь вид
\begin{equation*}
	\hat{X}(t) = \int\limits_{t_{0}}^{t} \psi(t,s) K(s) d y(s)  +  \psi(t,t_0) m_{0}.
\end{equation*}

\subsubsection{Дифференциальное уравнение для оценки $\hat{X}(t)$}
Запишем дифференциальное уравнение для полученной оценки (для вычислений дифференциальное представление может быть удобнее интегрального). Для этого продифференцируем предыдущее равенство с учетом входа переменной, по которой мы дифференцируем, в подынтегральное выражение.
\begin{multline*}
	d\hat{X}(t) = \psi(t,t) K(t) d y(t) + \left( \int\limits_{t_{0}}^{t} \frac{\partial \psi(t,s)}{\partial t} K(s) d y(s) + \frac{\partial \psi(t,t_0)}{\partial t} m_{0} \right)dt = \\
	= \text{ учтём, что $\psi(\cdot, \cdot)$ --- фундаментальная матрица } = \\
	= K(t)dy(t) + \left( \int\limits_{t_{0}}^{t} (A(t) - K(t)C(t))\psi(t,s) K(s) d y(s) + (A(t) - K(t)C(t))\psi(t,t_0) m_{0} \right)dt = \\
	= K(t)dy(t) + (A(t) - K(t)C(t))\hat{X}(t) dt
\end{multline*}

В результате получим уравнение вида
\begin{equation}
	d\hat{X}(t) = \underbrace{A(t)\hat{X}(t)dt}_{\text{шаг системы}} + \underbrace{K(t)(dy(t) - C(t)\hat{X}(t)dt)}_{\text{учет измерений}}.
\end{equation}
и начальное условие
\begin{equation*}
	\hat{X}(t_0) = m_0.
\end{equation*}

\begin{task}
	Проанализировать полученное уравнение и проинтерпретировать все его члены, имея ввиду исходные уравнения динамики и наблюдения
\begin{equation*}
	\left\{ \begin{aligned}
		dx & = Ax dt + dv, \\
		dy & = Cx dt + de,
	\end{aligned} \right.
\end{equation*}

	Сравнить полученное выражение с решением задачи фильтрации в дискретном случае.
\end{task}

\subsubsection{Вывод выражения для дисперсии ошибки}
Нам осталось разобраться со стохастическим смыслом матрицы $P(t)$. Покажем, что $P(t)$ имеет смысл дисперсии ошибки. Для этого выпишем, чему равен дифференциал ошибки (в формулу для дифференциала оценки мы подставили уравнение наблюдения)
\begin{equation*}
	d\tilde{x} = dx - d\hat{x} = (Axdt + dv) - (A\hat{x}dt + KC(x-\hat{x}))dt + Kde)
\end{equation*}
откуда после простейших преобразований получим
\begin{equation*}
	d\tilde{x} = (A(t) - K(t)C(t))\tilde{x}dt + dv(t) - K(t) de(t)
\end{equation*}
с начальным условием $\tilde{x}(t_0) = 0$.

Обозначим дисперсию ошибки $Q(t) = \E \tilde{x}(t)\tilde{x}^T(t)$ и запишем для неё дифференциально уравнение (тут будет финт ушами с бесконечно малыми величинами)
\begin{multline}
	dQ(t) = d(\E \tilde{x}(t)\tilde{x}^T(t)) = \E d(\tilde{x}(t)\tilde{x}^T(t)) = \\
	= \E \left\{ \tilde{x}(t+dt)\tilde{x}^T(t+dt) -  \tilde{x}(t)\tilde{x}^T(t) \right\} = \\
	= \E \left\{ d\tilde{x}(t)\tilde{x}^T(t) + \tilde{x}(t+dt)d\tilde{x}^T(t) \right\} = \\
	= \E \left\{ d\tilde{x}(t)\tilde{x}^T(t) + \tilde{x}(t)d\tilde{x}^T(t) + d\tilde{x}(t)d\tilde{x}^T(t) \right\} = \\
	= \text{\{ оставляем лишь то, что имеет порядок более $\bar{o}(dt), \; dv \sim de \sim \sqrt{dt}$ \}} = \\
	= (A(t) - K(t)C(t))Q(t) dt + Q(t)(A(t) - K(t)C(t))^T dt + \\ + R_1dt + K(t) R_2 K^T(t)dt + \bar{o}(dt)
\end{multline}
откуда получим итоговое уравнение
\begin{equation*}
\left\{ \begin{aligned}
	&\dot{Q} = (A-KC)Q + Q(A-KC)^T + R_1 + K R_2 K^T, \\
	&Q(t_0) = R_0.
\end{aligned} \right.	
\end{equation*}

Покажем, что решение полученного уравнения в точности совпадает с решением уравнения Рикатти для матрицы $P(t)$. Для этого запишем диффур для $Q-P$ (проделайте упрощения самостоятельно)
\begin{equation*}
\left\{ \begin{aligned}
	&\frac{d}{dt}(Q-P) = (A-P C^T R_2^{-1} C)(Q-P) + (Q-P)(A-P C^T R_2^{-1} C), \\
	&Q(t_0)-P(t_0) = 0.
\end{aligned} \right.	
\end{equation*}

Мы получили линейное однородное дифференциальное уравнение с нулевым начальным условием. У такого уравнения есть единственное нулевое решение. Следовательно,
\begin{equation*}
	P(t) \equiv Q(t) \;\; \text{ --- дисперсия ошибки оценивания}
\end{equation*}

Заметим, что $\E \tilde{x}(t) = 0 \;\; \forall t \in [t_0, t_1]$, что следует из равенства нулю в точке $t_0$ и однородности уравнения $\E d\tilde{x}(t) = (A-KC)dt \cdot \E\tilde{x}(t)$.

\subsection{Окончательная формулировка теоремы о решении задачи фильтрации}
\begin{theorem}
Линейная оптимальная в среднеквадратичном смысле оценка состояния системы
\begin{equation*}
	\left\{ \begin{aligned}
		dx & = Ax dt + dv, \\
		dy & = Cx dt + de,
	\end{aligned} \right.
\end{equation*}
где
\begin{align*}
	&x(t_{0}), e(t), v(t) \text{ --- независимы в совокупности}, \\
	&v(t), e(t) \text{ --- имеют независимые приращения}, \\
	&\E x(t_{0}) = m_{0}, \; \Var x(t_{0}) = R_{0}, \\
	&\E e(t) = \E v(t) = 0, \\
	&\E de (de)^{T} = R_{1} dt, \\
	&\E dv (dv)^{T} = R_{2} dt, \\
\end{align*}
удовлетворяет линейному стохастическому дифференциальному уравнению
\begin{equation*}
	\left\{ \begin{aligned}
		d \hat{X}(t) & = A(t) \hat{X}(t) dt + K(t)(dy(t) - C(t)\hat{X}(t) dt), \\
		\hat{X}(t_0) & = m_0 \text{ --- начальное условие},
	\end{aligned} \right.
\end{equation*}
где
\begin{equation*}
	K(t) = P(t) C^T(t) R_2^{-1},
\end{equation*}
а $P(t)$ --- ковариационная матрица ошибок, удовлетворяющая уравнению Рикатти:
\begin{equation*}
\left\{ \begin{aligned}
	&\dot{P} - PA^T + AP + P C^T R_2^{-1} P - R_1 = 0, \\
	&P(t_0) = R_0 \text{ --- начальное условие}.
\end{aligned} \right.	
\end{equation*}

При этом, состояние системы $X(t)$ является гауссовским случайным вектором с математическим ожиданием $\hat{X}(t)$ и дисперсией ошибки $\E (X(t)-\hat{X}(t))(X(t)-\hat{X}(t))^T = P(t)$.
\end{theorem}

%%%%%%%%%%%%%%%%%%%
% Вынести частную производную в одельную команду =)
